/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#include "stdafx.h"

#include "球形八叉树.h"
#include <线性代数/几何计算.h>




typedef struct {
	vec3			m_pos;
	vec3			m_边界;
	//球体空间里面表示半径
	vec3			m_空间大小;

	const float32	m_坐标半径;

	void* m_数据;
}S_子空间分配参数;



static S_八叉树子空间* f_创建子空间() {
	S_八叉树子空间* t = (S_八叉树子空间*)malloc(sizeof(S_八叉树子空间));
	//t->m_子集 = (S_八叉树子空间**)malloc(sizeof(S_八叉树子空间*) * 8);
	t->m_子集[0] = nullptr;
	t->m_子集[1] = nullptr;
	t->m_子集[2] = nullptr;
	t->m_子集[3] = nullptr;
	t->m_子集[4] = nullptr;
	t->m_子集[5] = nullptr;
	t->m_子集[6] = nullptr;
	t->m_子集[7] = nullptr;

	t->m_坐标 = {};
	t->m_数据 = nullptr;

	return t;
}



static void f_空间划分_add子集(S_八叉树子空间* t, S_子空间分配参数& 子空间分配参数) {
	uint8 mask = 0;
	uint8 进入子集 = 0;
	



	t->m_域大小 = 子空间分配参数.m_空间大小.x;

	//计算下一层空间大小
	if (子空间分配参数.m_空间大小.x > 子空间分配参数.m_坐标半径) {
		子空间分配参数.m_空间大小.x *= 0.5f;
	}
	else {
		子空间分配参数.m_空间大小.x = 0;
		进入子集 |= 0b0001;
	}

	if (子空间分配参数.m_空间大小.y > 子空间分配参数.m_坐标半径) {
		子空间分配参数.m_空间大小.y *= 0.5f;
	}
	else {
		子空间分配参数.m_空间大小.y = 0;
		进入子集 |= 0b0010;
	}

	if (子空间分配参数.m_空间大小.z > 子空间分配参数.m_坐标半径) {
		子空间分配参数.m_空间大小.z *= 0.5f;
	}
	else {
		子空间分配参数.m_空间大小.z = 0;
		进入子集 |= 0b0100;
	}
	




	//计算坐标所在空间象限
	if (子空间分配参数.m_pos.x >= 子空间分配参数.m_边界.x) {
		mask |= 0b00000000;

		子空间分配参数.m_边界.x += 子空间分配参数.m_空间大小.x;
	}
	else {
		mask |= 0b00000001;

		子空间分配参数.m_边界.x -= 子空间分配参数.m_空间大小.x;
	}

	if (子空间分配参数.m_pos.y >= 子空间分配参数.m_边界.y) {
		mask |= 0b00000000;

		子空间分配参数.m_边界.y += 子空间分配参数.m_空间大小.y;
	}
	else {
		mask |= 0b00000010;

		子空间分配参数.m_边界.y -= 子空间分配参数.m_空间大小.y;
	}

	if (子空间分配参数.m_pos.z >= 子空间分配参数.m_边界.z) {
		mask |= 0b00000000;
		子空间分配参数.m_边界.z += 子空间分配参数.m_空间大小.z;
	}
	else {
		mask |= 0b00000100;
		子空间分配参数.m_边界.z -= 子空间分配参数.m_空间大小.z;
	}




	
	if ( 7 != 进入子集 ) {
		if (nullptr == (t->m_子集[mask])) {
			t->m_子集[mask] = f_创建子空间();
		}

		t->m_子集[mask]->m_坐标 = 子空间分配参数.m_边界;

		f_空间划分_add子集(t->m_子集[mask], 子空间分配参数);
	}
	else {
		//子空间分配参数.m_数据 = t->m_子集[mask];
		子空间分配参数.m_数据 = t;
	}


	
}



/*template<typename T>
static void f_空间划分_get射线子集数据(S_八叉树子空间* t, const vec3& 起点, const vec3& dir, const S_VecSphere& 射线中点, float32 空间大小, std::vector<T>& data) {
	const float32 判断距离 = 空间大小 * 1.41421356237f;
	//vec3 射线方向 = 射线终点 - 射线起点;
	float32 距离 = vec_len(t->m_坐标 - 射线中点.pos);
	if (距离 >= 射线中点.radius ) {
		距离 = f_直线线到点的距离(起点, dir, { t->m_坐标, 判断距离 });
		
		if (距离 >= 0) {
			if (t->m_数据) data.push_back(t->m_数据);

			for (uint8 i = 0; i < 8; ++i) {
				if(t->m_子集[i]) f_空间划分_get射线子集数据(t->m_子集[i], 起点, dir, 射线中点, 空间大小 * 0.5f, data);
			}
		}
	}
}*/

static void f_空间划分_get射线子集数据(S_八叉树子空间* t, const vec3& 起点, const vec3& dir, const S_VecSphere& 射线中点, float32 空间大小, std::vector<S_八叉树子空间*>& data) {
	const float32 判断距离 = 空间大小 * 1.41421356237f;
	//vec3 射线方向 = 射线终点 - 射线起点;
	float32 距离 = vec_len(t->m_坐标 - 射线中点.pos);
	if (距离 >= 射线中点.radius) {
		距离 = f_直线到点的距离(起点, dir, { t->m_坐标, 判断距离 });

		if (距离 >= 0) {
			if(t->m_数据) data.push_back(t);

			for (uint8 i = 0; i < 8; ++i) {
				if (t->m_子集[i]) f_空间划分_get射线子集数据(t->m_子集[i], 起点, dir, 射线中点, 空间大小 * 0.5f, data);
			}
		}
	}
}


static void f_空间划分_get射线子集数据(S_八叉树子空间* t, const vec3& 起点, const vec3& dir, const S_VecSphere& 射线中点, float32 空间大小, std::vector<vec4>& data) {
	const float32 判断距离 = 空间大小 *  2.0;
	float32 距离 = f_直线到点的距离(起点, dir, { t->m_坐标, 判断距离 });
	
	if (距离 >= 0) {
		if (t->m_数据) data.push_back({ t->m_坐标.x, t->m_坐标.y, t->m_坐标.z, 空间大小 });

		for (uint8 i = 0; i < 8; ++i) {
			if (t->m_子集[i]) f_空间划分_get射线子集数据(t->m_子集[i], 起点, dir, 射线中点, 空间大小 * 0.5f, data);
		}
	}
}


static void f_空间划分_线段查空间(S_八叉树子空间* t, const vec3& 起点, const vec3& dir, const S_VecSphere& 射线中点, float32 空间大小, std::vector<S_八叉树子空间*>& data) {
	//const float32 判断距离 = 空间大小 * 1.41421356237f;
	const float32 判断距离 = 空间大小 * 2.0f;
	
	float32 距离 = vec_len(t->m_坐标 - 射线中点.pos);
	if (距离 <= 射线中点.radius + 判断距离) {
		//距离 = f_射线与点相交(起点, dir, { t->m_坐标, 判断距离 });
		距离 = f_直线到点的距离(起点, dir, { t->m_坐标, 判断距离 });

		if (距离 >= 0) {
			if (t->m_数据) data.push_back(t);
			//data.push_back(t);
			for (uint8 i = 0; i < 8; ++i) {
				if (t->m_子集[i]) f_空间划分_线段查空间(t->m_子集[i], 起点, dir, 射线中点, 空间大小 * 0.5f, data);
			}
		}
	}
}


template<typename T>
static void	f_空间划分_get所有子集数据(S_八叉树子空间* t, std::vector<T*>& data) {
	if (t->m_数据) data.push_back(t->m_数据);

	for (uint8 i = 0; i < 8; ++i) {
		if (t->m_子集[i]) f_空间划分_get所有子集数据(t->m_子集[i], data);
	}
}

/*template<typename T>
static void	f_空间划分_get范围子集数据(S_八叉树子空间* t, const S_VecSphere& 范围, float32 空间大小, std::vector<T*>& data) {
	const float32 相隔距离 = vec_len(范围.pos - t->m_坐标);
	if (范围.radius >= 相隔距离) {
		if (t->m_数据) data.push_back(t->m_数据);

		for (uint8 i = 0; i < 8; ++i) {
			if(t->m_子集[i]) f_空间划分_get所有子集数据(t->m_子集[i], data);
		}

		return;
	}

	if (相隔距离 <= 空间大小 * 1.735f) {
		if(t->m_数据) data.push_back(t->m_数据);

		for (uint8 i = 0; i < 8; ++i) {
			if (t->m_子集[i]) f_空间划分_get范围子集数据(t->m_子集[i], 范围, 空间大小 * 0.5f, data);
		}
	}
}*/



static void	f_空间划分_get所有子集数据(S_八叉树子空间* t, std::vector<S_八叉树子空间*>& data) {
	data.push_back(t);
	for (uint8 i = 0; i < 8; ++i) {
		if (t->m_子集[i]) f_空间划分_get所有子集数据(t->m_子集[i], data);
	}
}

static void	f_空间划分_get范围子集单元(S_八叉树子空间* t, const S_VecSphere& 范围, float32 空间大小, std::vector<S_八叉树子空间*>& data) {
	const float32 相隔距离 = vec_len(范围.pos - t->m_坐标);
	//1.735f
	//空间大小 *= 0.5f;
	if (相隔距离 <= 空间大小 * 1.735f + 范围.radius) {
		data.push_back(t);

		for (uint8 i = 0; i < 8; ++i) {
			if (t->m_子集[i]) f_空间划分_get范围子集单元(t->m_子集[i], 范围, 空间大小 * 0.5f, data);
		}
	}

	
}

static void	f_空间划分_get范围子集数据(S_八叉树子空间* t, const S_VecSphere& 范围, float32 空间大小, std::vector<S_八叉树子空间*>& data) {
	const float32 相隔距离 = vec_len(范围.pos - t->m_坐标);
	//1.735f
	//空间大小 *= 0.5f;
	if (相隔距离 <= 空间大小 * 1.735f + 范围.radius) {
		if(t->m_数据) data.push_back(t);

		for (uint8 i = 0; i < 8; ++i) {
			if (t->m_子集[i]) f_空间划分_get范围子集数据(t->m_子集[i], 范围, 空间大小 * 0.5f, data);
		}
	}
}



static void	f_空间划分_移除所有子集(S_八叉树子空间* t) {
	for (uint8 i = 0; i < 8; ++i) {
		if (t->m_子集[i]) {
			f_空间划分_移除所有子集(t->m_子集[i]);
			t->m_子集[i] = nullptr;
		}
	}
	free(t);
}








S_点距八叉树* f_空间划分_create球形八叉树(float32 空间大小) {
	S_点距八叉树* obj = (S_点距八叉树*)malloc(sizeof(S_点距八叉树));

	(*obj) = {};
	obj->m_空间大小 = 空间大小;
	obj->m_最小单元大小 = 0.5;

	obj->m_空间 = f_创建子空间();
	obj->m_映射的数据类型 = E_值类型::e_Type_None;
	obj->m_映射的数据 = nullptr;

	return obj;
}

void f_空间划分_destroy球形八叉树(S_点距八叉树* t) {
	f_空间划分_remove所有子集(t);
	free(t);
}

void f_空间划分_add子集(S_点距八叉树* t, const vec3& pos, const float32 半径, void* data) {
	S_子空间分配参数 分配参数 = { pos , {0,0,0}, {t->m_空间大小, t->m_空间大小, t->m_空间大小}, 半径, data };

	f_空间划分_add子集(t->m_空间, 分配参数);
	//for (uint8 i = 0; i < 8; ++i) {
	//	f_空间划分_add子集(t->m_空间->m_子集[i], 分配参数);
	//}
}

S_八叉树子空间* f_空间划分_add子集(S_点距八叉树* t, const vec3& pos, const float32 半径) {
	S_子空间分配参数 分配参数 = { pos , {0,0,0}, {t->m_空间大小, t->m_空间大小, t->m_空间大小}, 半径, nullptr };
	f_空间划分_add子集(t->m_空间, 分配参数);
	return (S_八叉树子空间*)分配参数.m_数据;
}

void f_空间划分_从包围盒构建子集(S_点距八叉树* t, const std::vector<S_VecSphere>& 包围盒) {
	f_空间划分_remove所有子集(t);

	auto* box = 包围盒.data();
	uint32 num = 包围盒.size();

	for (uint32 三角形面索引 = 0; 三角形面索引 < num; ++三角形面索引) {
		auto* 单元 = f_空间划分_add子集(t, box[三角形面索引].pos, box[三角形面索引].radius);
		if (单元->m_数据 == nullptr) {
			单元->m_数据 = new std::vector<uint32>();
		}
		((std::vector<uint32>*)单元->m_数据)->push_back(三角形面索引);
	}

	t->m_映射的数据类型 = E_值类型::e_Type_UI32_1D;
}

void f_空间划分_remove子集(S_点距八叉树* t, const vec3& pos) {

}

void f_空间划分_remove所有子集(S_点距八叉树* t) {
	for (uint8 i = 0; i < 8; ++i) {
		if (t->m_空间->m_子集[i]) {
			f_空间划分_移除所有子集(t->m_空间->m_子集[i]);
			t->m_空间->m_子集[i] = nullptr;
		}
	}
	
}



void f_空间划分_get位置子集数据(S_八叉树子空间* 子空间, const vec3& pos, std::vector<void*>& data) {
	if (vec_len(子空间->m_坐标 - pos) <= 子空间->m_域大小*2 && 子空间->m_数据) {
		data.push_back(子空间->m_数据);
	}
	for (uint8 i = 0; i < 8; ++i) {
		if (子空间->m_子集[i]) {
			f_空间划分_get位置子集数据(子空间->m_子集[i], pos, data);
		}
	}
}

void f_空间划分_get位置子集数据(S_点距八叉树* t, const vec3& pos, std::vector<void*>& data) {
	f_空间划分_get位置子集数据(t->m_空间, pos, data);
}

/*void f_空间划分_get射线子集数据(S_点距八叉树* t, const vec3& 开始, const vec3& 结束, std::vector<void*>& data) {
	vec3 dir = 结束 - 开始;
	vec3 射线中点 = dir * 0.5f + 开始;
	float32 射线一半长度 = vec_len(dir) * 0.5f;

	vec_normalize(&dir);

	for (uint8 i = 0; i < 8; ++i) {
		if (t->m_空间->m_子集[i]) f_空间划分_get射线子集数据(t->m_空间->m_子集[i], 开始, dir, { 射线中点, 射线一半长度 }, t->m_空间大小, data);
	}
}*/

void f_空间划分_get射线子集数据(S_点距八叉树* t, const vec3& 开始, const vec3& 结束, std::vector<S_八叉树子空间*>& data) {
	vec3 dir = 结束 - 开始;
	vec3 射线中点 = dir * 0.5f + 开始;
	float32 射线一半长度 = vec_len(dir) * 0.5f;

	vec_normalize(&dir);

	for (uint8 i = 0; i < 8; ++i) {
		if (t->m_空间->m_子集[i]) f_空间划分_get射线子集数据(t->m_空间->m_子集[i], 开始, dir, { 射线中点, 射线一半长度 }, t->m_空间大小, data);
	}
}




void	f_空间划分_线段查子空间(const S_点距八叉树* t, const S_线段& line, std::vector<S_八叉树子空间*>& data) {
	vec3 dir = line.end - line.begin;
	vec3 射线中点 = dir * 0.5f + line.begin;
	float32 射线一半长度 = vec_len(dir) * 0.5f;
	//float32 射线一半长度 = vec_len(dir);

	vec_normalize(&dir);

	for (uint8 i = 0; i < 8; ++i) {
		if (t->m_空间->m_子集[i]) f_空间划分_线段查空间(t->m_空间->m_子集[i], line.begin, dir, { 射线中点 , 射线一半长度 }, t->m_空间大小 * 0.5f, data);
	}
}

/*void f_空间划分_get范围子集数据(const S_点距八叉树* t, const S_VecSphere& 范围, std::vector<void*>& data) {
	for (uint8 i = 0; i < 8; ++i) {
		if(t->m_空间->m_子集[i]) f_空间划分_get范围子集单元(t->m_空间->m_子集[i], 范围, t->m_空间大小 * 0.5f, data);
	}
}*/

void f_空间划分_get范围子集数据(const S_点距八叉树* t, const S_VecSphere& 范围, std::vector<S_八叉树子空间*>& data, bool 是否包含数据) {
	if (是否包含数据) {
		for (uint8 i = 0; i < 8; ++i) {
			if (t->m_空间->m_子集[i]) f_空间划分_get范围子集数据(t->m_空间->m_子集[i], 范围, t->m_空间大小 * 0.5f, data);
		}
	}
	else {
		for (uint8 i = 0; i < 8; ++i) {
			if (t->m_空间->m_子集[i]) f_空间划分_get范围子集单元(t->m_空间->m_子集[i], 范围, t->m_空间大小 * 0.5f, data);
		}
	}
}

void f_空间划分_set叉树空间大小(S_点距八叉树* t, float32 空间大小, vec3 offset) {
	t->m_空间大小 = 空间大小;
	t->m_空间偏移 = offset;

	f_空间划分_remove所有子集(t);
}

void f_空间划分_从坐标索引(S_点距八叉树* t, const std::vector<S_VecSphere>& 包围盒) {
	auto* box = 包围盒.data();
	uint32 num = 包围盒.size();

	for (uint32 三角形面索引 = 0; 三角形面索引 < num; ++三角形面索引) {
		auto* 单元 = f_空间划分_add子集(t, box[三角形面索引].pos, box[三角形面索引].radius);
		if (单元->m_数据 == nullptr) {
			单元->m_数据 = new std::vector<uint32>();
		}
		((std::vector<uint32>*)单元->m_数据)->push_back(三角形面索引);
	}

	t->m_映射的数据类型 = E_值类型::e_Type_UI32_1D;
}









static void f_空间划分_转换为坐标(const S_八叉树子空间* t, std::vector<vec4>& pos, vec3 空间大小, vec4 起始坐标, bool 是否包含数据) {
	vec_mul(&空间大小, 0.5);
	
	pos.reserve(pos.size() + 8);

	vec4 最终坐标 = {};
	起始坐标.w = 空间大小.x;
	for (uint8 i = 0; i < 8; ++i) {
		if (nullptr != (t->m_子集[i])) {

			最终坐标.x = t->m_子集[i]->m_坐标.x;
			最终坐标.y = t->m_子集[i]->m_坐标.y;
			最终坐标.z = t->m_子集[i]->m_坐标.z;
			最终坐标.w = 空间大小.x;
			
			if (是否包含数据) {
				if(t->m_子集[i]->m_数据) pos.push_back(最终坐标);
			}
			else {
				pos.push_back(最终坐标);
			}
			
			f_空间划分_转换为坐标(t->m_子集[i], pos, 空间大小, 最终坐标, 是否包含数据);
		}
	}
}

static void f_空间划分_有数据单元转换为坐标(const S_八叉树子空间* t, std::vector<vec4>& pos, vec3 空间大小, vec4 起始坐标) {
	vec_mul(&空间大小, 0.5);

	pos.reserve(pos.size() + 8);

	vec4 最终坐标 = {};
	起始坐标.w = 空间大小.x;
	for (uint8 i = 0; i < 8; ++i) {
		if (nullptr != (t->m_子集[i])) {

			最终坐标.x = t->m_子集[i]->m_坐标.x;
			最终坐标.y = t->m_子集[i]->m_坐标.y;
			最终坐标.z = t->m_子集[i]->m_坐标.z;
			最终坐标.w = 空间大小.x;

			if (t->m_子集[i]->m_数据) {
				pos.push_back(最终坐标);
			}
			
			f_空间划分_有数据单元转换为坐标(t->m_子集[i], pos, 空间大小, 最终坐标);
		}
	}
}


void f_空间划分_解析为坐标(const S_点距八叉树* t, std::vector<vec4>& pos, bool 是否包含数据) {
	f_空间划分_转换为坐标(t->m_空间, pos, { t->m_空间大小, t->m_空间大小, t->m_空间大小 }, {}, 是否包含数据);

	if (t->m_映射的数据类型 == E_值类型::e_Type_UI32_1D) {

	}
}

void f_空间划分_单元解析为坐标(const S_点距八叉树* t, std::vector<vec4>& pos) {
	f_空间划分_有数据单元转换为坐标(t->m_空间, pos, { t->m_空间大小, t->m_空间大小, t->m_空间大小 }, {});
}



void f_空间划分_射线拾取结果解析为域坐标(S_点距八叉树* t, vec3 开始, const vec3& 结束, std::vector<vec4>& data) {
	vec3 dir = 结束 - 开始;
	vec3 射线中点 = dir * 0.5f + 开始;
	float32 射线一半长度 = vec_len(dir) * 0.5f;

	vec_normalize(&dir);
	//vec3 起点逆行方向 = vec_反转(dir);
	//开始 = 开始 - (dir * t->m_空间大小);
	for (uint8 i = 0; i < 8; ++i) {
		if(t->m_空间->m_子集[i]) f_空间划分_get射线子集数据(t->m_空间->m_子集[i], 开始, dir, { 射线中点, 射线一半长度 }, t->m_空间大小*0.5, data);
	}

}









static void	f_空间划分_get所有子集坐标(S_八叉树子空间* t, float32 空间大小, std::vector<vec4>& data) {
	vec4 最终坐标 = {};
	最终坐标.x = t->m_坐标.x;
	最终坐标.y = t->m_坐标.y;
	最终坐标.z = t->m_坐标.z;
	最终坐标.w = 空间大小;

	data.push_back(最终坐标);

	for (uint8 i = 0; i < 8; ++i) {
		if (t->m_子集[i]) f_空间划分_get所有子集坐标(t->m_子集[i], 空间大小 * 0.5f, data);
	}

}

static void	f_空间划分_get范围子集坐标(S_八叉树子空间* t, const S_VecSphere& 范围, float32 空间大小, std::vector<vec4>& data) {
	const float32 相隔距离 = vec_len(范围.pos - t->m_坐标);

	vec4 最终坐标 = {};
	最终坐标.x = t->m_坐标.x;
	最终坐标.y = t->m_坐标.y;
	最终坐标.z = t->m_坐标.z;
	最终坐标.w = 空间大小;

	if (范围.radius >= 相隔距离) {
		data.push_back(最终坐标);

		for (uint8 i = 0; i < 8; ++i) {
			if (t->m_子集[i]) f_空间划分_get所有子集坐标(t->m_子集[i], 空间大小 * 0.5f, data);
		}
		return;
	}

	if (相隔距离 <= 空间大小 * 1.735f) {
		data.push_back(最终坐标);

		for (uint8 i = 0; i < 8; ++i) {
			if (t->m_子集[i]) f_空间划分_get范围子集坐标(t->m_子集[i], 范围, 空间大小 * 0.5f, data);
		}
	}
}

void f_空间划分_范围拾取解析为域坐标(S_点距八叉树* t, const S_VecSphere& 范围, std::vector<vec4>& data) {
	for (uint8 i = 0; i < 8; ++i) {
		if(t->m_空间->m_子集[i]) f_空间划分_get范围子集坐标(t->m_空间->m_子集[i], 范围, t->m_空间大小 * 0.5f, data);
	}
}











